
#include "user_it_callback.h"
#include "math.h"

uint8_t FOC_MODE=0;//默认情况下为位置模式

uint16_t Sampling_Angle;//采样角度

float Sampling_Speed_NoCom;//采样速度(未计算校准)

float want_speed=0.0;//期望速度角(0~1) 单位：2PI
float want_position=2048;//对PID输入的期望转子位置 初始化位置 位置闭环

float Phase_Error=0;//相位误差 全局变量

float EEtime;//控制频率

uint8_t Moto_Direction=1;//电机旋转方向标注 全局 用于基础驱动环节

/*用户初始化程序 完成板级外设的初始化*/
void user_init(void)
{
	  Moto_Strat(&MOTO_TIM);
	  OLED_ShowString(3, 1, "MOTO_Init");
	  Moto_Pash_Calibration();
	  OLED_ShowString(3, 1, "MOTO_OK");
	//  Open_Music();
	  EEtime=Execute_Cycle(&htim3);
	  low_pass_filter_init();
//	  HAL_I2C_Mem_Read(&hi2c1, 0xa0, 0x00, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&POSE_PIDStruct.Kp, 4, 0xffff);
//	  HAL_Delay(10);
//	  HAL_I2C_Mem_Read(&hi2c1, 0xa0, 0x05, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&POSE_PIDStruct.Ki, 4, 0xffff);
//	  HAL_Delay(10);
//	  HAL_I2C_Mem_Read(&hi2c1, 0xa0, 0x0a, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&POSE_PIDStruct.Kd, 4, 0xffff);
//	  HAL_Delay(10);
//	  HAL_I2C_Mem_Read(&hi2c1, 0xa0, 0x10, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&SPEED_PIDSreuct.Kp, 4, 0xffff);
//	  HAL_Delay(10);
//	  HAL_I2C_Mem_Read(&hi2c1, 0xa0, 0x15, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&SPEED_PIDSreuct.Ki, 4, 0xffff);
//	  HAL_Delay(10);
//	  HAL_I2C_Mem_Read(&hi2c1, 0xa0, 0x1a, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&SPEED_PIDSreuct.Kd, 4, 0xffff);
//	  HAL_Delay(10);
	  //open UART interrupt
	  HAL_UART_Receive_IT(&huart1,(uint8_t*)&VOFA_RECE_Struct, 8);
	  //open TIM task
	  HAL_TIM_Base_Start_IT(&htim3);
	  HAL_TIM_Base_Start_IT(&htim4);
	  HAL_TIM_Base_Start_IT(&htim5);
}

/*
 * 开机音乐
 * */
void Open_Music(void)
{
	Moto_EN;

	TIM2->PSC=68;//do 1
	HAL_Delay(250);

	TIM2->PSC=61;//mi 2
	HAL_Delay(250);

	TIM2->PSC=46;//so 5
	HAL_Delay(500);

	MX_TIM2_Init();//重新初始化
	Moto_DIS;
}

/*
 * @berif: 计算执行调节周期
 * @parm:	输入定时器句柄
 * @return 返回单位为秒/S
 * */
float Execute_Cycle(TIM_HandleTypeDef * htim)
{
	float Etime;
	Etime = ((htim->Init.Prescaler+1) * htim->Init.Period)/72000.0;//计算周期
	return Etime;
}
/**
 * @name: 电机相位误差校准
 * 初始化任务 一次执行
 */
void Moto_Pash_Calibration(void)
{
	float Expected_Angle;//扇区内角度
	Moto_EN;
	rtU.reg=0;//给定0位置;
	foc_step();//foc驱动计算函数
	Moto_Value_Conversion(&MOTO_UV,&rtY);
	Moto_Set_PWM(&MOTO_TIM,&MOTO_UV);
	HAL_Delay(2000);
	/*设置位置*/
	Sampling_Angle=i2c_AS5600_get_angle();//获取绝对位置
	Expected_Angle = Sampling_Angle % Sector_Width;//扇区分割
	Phase_Error = Expected_Angle/Sector_Width;//比例换算后赋给其相位误差
	Moto_DIS;
}

/*
 * FOC动力驱动执行 频率决定运动是否流畅
 * 这个程序主要负责驱动无刷电机的换向
 * */
void FOC_Driver_Updates(void)
{
	/*获取编码器值并且换算分割*/
	float Expected_Angle;//扇区内角度

	Expected_Angle = Sampling_Angle % Sector_Width;//扇区分割,电角度换算为机械角度，电机有x对磁极
	Expected_Angle = Expected_Angle/Sector_Width;//比例换算 电角度内占了2PI rad的百分之多少
	Expected_Angle -= Phase_Error;//消去相位误差得到当前扇区的绝对位置

	/****修改运动位置等效运动矢量****/
	if(Moto_Direction)
	{
	rtU.reg=(Expected_Angle+(0.33));//周期内期望向量的模
	}
	else
	{
	rtU.reg=(Expected_Angle-(0.33));
	}
	/************/
	foc_step();//foc驱动计算函数
	/************/
	Moto_Value_Conversion(&MOTO_UV,&rtY);
	Moto_Set_PWM(&MOTO_TIM,&MOTO_UV);

}


/*定时任务的回调处理区域*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
		//角度更新任务 执行频率：72M/(720 * 100)=1kHz
		if(htim==&htim3)
		{
			Sampling_Angle=i2c_AS5600_get_angle();
			Sampling_Speed_NoCom=low_pass_filter((-Encoder_Speed(Sampling_Angle,EEtime)));//这个值乘以执行频率（缩小1000倍后的频率）除编码器分辨率 就是每秒钟的旋转次数
			FOC_Driver_Updates();
		}

		if(htim==&htim5)//PID控制任务
		{
			if(FOC_MODE==0)//串口位置模式
			{
				FOC_Nearest_PositionMode(want_position,Sampling_Angle);
			}
			if(FOC_MODE==1)
			{
				//rtU.q=8;
				FOC_SpeedCloseloop(want_speed,Sampling_Speed_NoCom);
			}
			if(FOC_MODE==2)
			{
				Shift_Knob(Sampling_Angle);
			}
		}
		//串口定时发送任务
		if(htim==&htim4)//发送频率为1ms
		{
			if(FOC_MODE==0)//位置模式
			{
			VOFA_SEND_Struct.fdata[0]=POSE_PIDStruct.Kp;
			VOFA_SEND_Struct.fdata[1]=POSE_PIDStruct.Ki;
			VOFA_SEND_Struct.fdata[2]=POSE_PIDStruct.Kd;
			VOFA_SEND_Struct.fdata[3]=POSE_PIDStruct.out;//PID输出
			VOFA_SEND_Struct.fdata[4]=want_position;//期望位置
			VOFA_SEND_Struct.fdata[5]=Sampling_Angle;//采样角
			VOFA_SEND_Struct.fdata[6]=Sampling_Angle/11.375;
			VOFA_SEND_Struct.fdata[7]=Sampling_Speed_NoCom;
			}
			else if(FOC_MODE==1)
			{
			VOFA_SEND_Struct.fdata[0]=SPEED_PIDSreuct.Kp;
			VOFA_SEND_Struct.fdata[1]=SPEED_PIDSreuct.Ki;
			VOFA_SEND_Struct.fdata[2]=SPEED_PIDSreuct.Kd;
			VOFA_SEND_Struct.fdata[3]=SPEED_PIDSreuct.out;//PID输出
			VOFA_SEND_Struct.fdata[4]=want_speed;//期望位置
			VOFA_SEND_Struct.fdata[5]=Sampling_Speed_NoCom;//采样角
			VOFA_SEND_Struct.fdata[6]=Sampling_Angle/11.375;//动画演示角度
			VOFA_SEND_Struct.fdata[7]=Sampling_Speed_NoCom;
			}
			VOFA_SEND(&VOFA_SEND_Struct);
		}

}
/*串口中断回调*/
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if(huart==&huart1)
	{
		if(FOC_MODE==0)
		{
			switch(VOFA_RECE_Struct.UPSOFE_RX_ADDBUFF[3])
			{
				case 0x01:want_position=VOFA_RECE_Struct.f_data;break;//获取用户位置
				case 0x02:want_speed=VOFA_RECE_Struct.f_data;break;
				case 0x04:	{//模式切换
								if(VOFA_RECE_Struct.f_data==0.0f)
								{
									FOC_MODE=0;
								}
								else if(VOFA_RECE_Struct.f_data==1.0f)
								{
									FOC_MODE=1;
								}
								else if(VOFA_RECE_Struct.f_data==2.0f)
								{
									FOC_MODE=2;
								}
								break;
							}
				case 0x05:	{
								if(VOFA_RECE_Struct.f_data==0.0f)//如果值为0则关闭
								{HAL_GPIO_WritePin(MOTO_EN_GPIO_Port, MOTO_EN_Pin, GPIO_PIN_RESET);}//使能电机
								else
								{HAL_GPIO_WritePin(MOTO_EN_GPIO_Port, MOTO_EN_Pin, GPIO_PIN_SET);}
							}
							break;
				case 0x11:	POSE_PIDStruct.Kp=VOFA_RECE_Struct.f_data;\
							HAL_I2C_Mem_Write(&hi2c1, 0xa0, 0x00, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&POSE_PIDStruct.Kp, 4, 0xffff);
							break;
				case 0x12:POSE_PIDStruct.Ki=VOFA_RECE_Struct.f_data;\
							HAL_I2C_Mem_Write(&hi2c1, 0xa0, 0x05, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&POSE_PIDStruct.Ki, 4, 0xffff);
							break;
				case 0x13:POSE_PIDStruct.Kd=VOFA_RECE_Struct.f_data;\
							HAL_I2C_Mem_Write(&hi2c1, 0xa0, 0x0a, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&POSE_PIDStruct.Kd, 4, 0xffff);
							break;
				}
		}
		else if(FOC_MODE==1)
		{
			switch(VOFA_RECE_Struct.UPSOFE_RX_ADDBUFF[3])
			{
				case 0x01:want_position=VOFA_RECE_Struct.f_data;break;//获取用户位置
				case 0x02:want_speed=VOFA_RECE_Struct.f_data;break;
				case 0x04:	{//模式切换
								if(VOFA_RECE_Struct.f_data==0.0f)
								{
									FOC_MODE=0;
								}
								else if(VOFA_RECE_Struct.f_data==1.0f)
								{
									FOC_MODE=1;
								}
								else if(VOFA_RECE_Struct.f_data==2.0f)
								{
									FOC_MODE=2;
								}
								break;
							}
				case 0x05:	{
								if(VOFA_RECE_Struct.f_data==0.0f)//如果值为0则关闭
								{HAL_GPIO_WritePin(MOTO_EN_GPIO_Port, MOTO_EN_Pin, GPIO_PIN_RESET);}//使能电机
								else
								{HAL_GPIO_WritePin(MOTO_EN_GPIO_Port, MOTO_EN_Pin, GPIO_PIN_SET);}
							}
							break;
				case 0x11:	SPEED_PIDSreuct.Kp=VOFA_RECE_Struct.f_data;\
							HAL_I2C_Mem_Write(&hi2c1, 0xa0, 0x10, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&POSE_PIDStruct.Kp, 4, 0xffff);
							break;
				case 0x12:SPEED_PIDSreuct.Ki=VOFA_RECE_Struct.f_data;\
							HAL_I2C_Mem_Write(&hi2c1, 0xa0, 0x15, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&POSE_PIDStruct.Ki, 4, 0xffff);
							break;
				case 0x13:SPEED_PIDSreuct.Kd=VOFA_RECE_Struct.f_data;\
							HAL_I2C_Mem_Write(&hi2c1, 0xa0, 0x1a, I2C_MEMADD_SIZE_8BIT,(uint8_t *)&POSE_PIDStruct.Kd, 4, 0xffff);
							break;
				}
		}
	}


	HAL_UART_Receive_IT(&huart1,(uint8_t*)&VOFA_RECE_Struct,8);
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(GPIO_Pin==GPIO_PIN_4)
	{
		HAL_GPIO_TogglePin(MOTO_EN_GPIO_Port, MOTO_EN_Pin);
	}
	if(GPIO_Pin==GPIO_PIN_5)
	{
		FOC_MODE++;
		if(FOC_MODE>3)
		{
			FOC_MODE=0;
		}
	}

}
void main_app(void)
{
}



